#include "cartesian_to_frenet.h"


/// @brief 
/// @param a 
/// @param b 
/// @return 
double CartesianToFrenet::distance(const Point& a, const Point& b) {
    return std::hypot(a.x - b.x, a.y - b.y);
}
/// @brief 
/// @param a 
/// @param b 
/// @param p 
/// @return 
Point CartesianToFrenet::getProjection(const Point& a, const Point& b, const Point& p) {
    Point projection;
    double length_square = distance(a, b) * distance(a, b);
    
    if (length_square < std::numeric_limits<double>::epsilon()) {
        // Line segment is too short
        return a;
    }
    
    double u = ((p.x - a.x) * (b.x - a.x) + (p.y - a.y) * (b.y - a.y)) / length_square;
    
    if (u < 0.0) {
        projection = a; // Closer to the first point
    } else if (u > 1.0) {
        projection = b; // Closer to the second point
    } else {
        projection.x = a.x + u * (b.x - a.x);
        projection.y = a.y + u * (b.y - a.y);
    }
    
    return projection;
}
/// @brief 
/// @param xy_points 
/// @param xy 
/// @param s 
/// @param d 
/// @param index 
void CartesianToFrenet::cartesianToFrenet(const std::vector<std::pair<double, double>>& xy_points, Point& xy, double& s, double& d, int& index) {
    std::vector<Point> path(xy_points.size());
    for (size_t i = 0; i < xy_points.size(); ++i) {
        path[i].x = xy_points[i].first;
        path[i].y = xy_points[i].second;
    }

    Point p;
    p.x = xy.x;
    p.y = xy.y;

    double min_dist = std::numeric_limits<double>::max();
    Point closest_projection;
    int closest_segment_index = 0;

    // Iterate over each segment in the path to find the closest projection
    for (int i = 1; i < index + 25; ++i) {
        Point projection = getProjection(path[i-1], path[i], p);
        double dist = distance(p, projection);

        if (dist < min_dist) {
            min_dist = dist;
            closest_projection = projection;
            closest_segment_index = i;
        }
    }

    // Calculate the longitudinal coordinate 's' along the path
    s = 0.0;
    for (int j = 0; j < closest_segment_index - 1; ++j) {
        s += distance(path[j], path[j+1]);
    }
    s += distance(path[closest_segment_index - 1], closest_projection);

    // Calculate the lateral coordinate 'd' as the perpendicular distance from the path
    double dx = p.x - closest_projection.x;
    double dy = p.y - closest_projection.y;

    // Determine the sign of 'd' based on the cross product
    Point path_direction = { path[closest_segment_index].x - path[closest_segment_index - 1].x, path[closest_segment_index].y - path[closest_segment_index - 1].y };
    double cross_product = path_direction.x * dy - path_direction.y * dx;

    d = (cross_product >= 0) ? min_dist : -min_dist;
}
/// @brief 
/// @param xy_points 
/// @param xy 
/// @param s 
/// @param d 
void CartesianToFrenet::cartesian_Frenet(const std::vector<std::pair<double, double>>& xy_points, Point& xy, double& s, double& d) {
    std::vector<Point> path(xy_points.size());
    for (size_t i = 0; i < xy_points.size(); ++i) {
        path[i].x = xy_points[i].first;
        path[i].y = xy_points[i].second;
    }

    Point p;
    p.x = xy.x;
    p.y = xy.y;

    double min_dist = std::numeric_limits<double>::max();
    Point closest_projection;
    int closest_segment_index = 0;

    // Iterate over each segment in the path to find the closest projection
    for (int i = 1; i < path.size(); ++i) {
        Point projection = getProjection(path[i-1], path[i], p);
        double dist = distance(p, projection);

        if (dist < min_dist) {
            min_dist = dist;
            closest_projection = projection;
            closest_segment_index = i;
        }
    }

    // Calculate the longitudinal coordinate 's' along the path
    s = 0.0;
    for (int j = 0; j < closest_segment_index - 1; ++j) {
        s += distance(path[j], path[j+1]);
    }
    s += distance(path[closest_segment_index - 1], closest_projection);

    // Calculate the lateral coordinate 'd' as the perpendicular distance from the path
    double dx = p.x - closest_projection.x;
    double dy = p.y - closest_projection.y;

    // Determine the sign of 'd' based on the cross product
    Point path_direction = { path[closest_segment_index].x - path[closest_segment_index - 1].x, path[closest_segment_index].y - path[closest_segment_index - 1].y };
    double cross_product = path_direction.x * dy - path_direction.y * dx;

    d = (cross_product >= 0) ? min_dist : -min_dist;
}
/// @brief 
/// @param glob_path 
/// @param s 
/// @param d 
/// @param accumulated_s 
/// @param vehicle_index 
/// @return 
std::pair<double, double>  CartesianToFrenet::ConvertToCartesian(const std::vector<std::pair<double, double>>& glob_path, double s, double d,
                                                                                                                                            std::vector<double>& accumulated_s, int& vehicle_index) 
{
    // Step 1: 参考点
    // std::cout << "vehicle_index: " << vehicle_index  << std::endl;
    // std::cout << "accumulated_s: " << accumulated_s[vehicle_index]  << std::endl;

    // Step 2: 计算参考点的方向
    double dx = glob_path[vehicle_index + 1].first - glob_path[vehicle_index].first;
    double dy = glob_path[vehicle_index + 1].second - glob_path[vehicle_index].second;
    double theta = std::atan2(dx, dy);  // 参考点的切线方向

    // Step 3: 基于偏移量 d 计算全局坐标
    double remaining_s = s - accumulated_s[vehicle_index];
    double ref_x = glob_path[vehicle_index].first + remaining_s * std::cos(theta);
    double ref_y = glob_path[vehicle_index].second + remaining_s * std::sin(theta);

    // 偏移量 (d) 在垂直于路径方向的方向上
    double cartesian_x = ref_x - d * std::sin(theta);
    double cartesian_y = ref_y + d * std::cos(theta);

    return {cartesian_x, cartesian_y};
}
